#include "lidar_cells_grid.h"
namespace perception
{
    namespace algorithm
    {
        int LidarCellsGrid::Grid(int x, int y, float pz, float px, float py, std::vector<std::vector<data_cell>> &data_cell_)
        {
            data_cell_[x][y].x = x * cell_config_.cell_size_x + cell_config_.xmin + cell_config_.cell_size_x / 2;
            data_cell_[x][y].y = y * cell_config_.cell_size_y + cell_config_.ymin + cell_config_.cell_size_y / 2;
            data_cell_[x][y].idx = x * col + y;
            // 根据点云数量初始化xyz最大最小值
            if (data_cell_[x][y].point_num > 0)
            {
                data_cell_[x][y].z_max = pz > data_cell_[x][y].z_max ? pz : data_cell_[x][y].z_max;
                data_cell_[x][y].z_min = pz < data_cell_[x][y].z_min ? pz : data_cell_[x][y].z_min;
                data_cell_[x][y].x_max = px > data_cell_[x][y].x_max ? px : data_cell_[x][y].x_max;
                data_cell_[x][y].x_min = px < data_cell_[x][y].x_min ? px : data_cell_[x][y].x_min;
                data_cell_[x][y].y_max = py > data_cell_[x][y].y_max ? py : data_cell_[x][y].y_max;
                data_cell_[x][y].y_min = py < data_cell_[x][y].y_min ? py : data_cell_[x][y].y_min;
            }
            else
            {
                data_cell_[x][y].z_max = pz;
                data_cell_[x][y].z_min = pz;
                data_cell_[x][y].y_max = py;
                data_cell_[x][y].y_min = py;
                data_cell_[x][y].x_max = px;
                data_cell_[x][y].x_min = px;
            }
            data_cell_[x][y].sum += pz;
            data_cell_[x][y].point_num++;
            data_cell_[x][y].z_intercept = data_cell_[x][y].z_max - data_cell_[x][y].z_min;
            data_cell_[x][y].z_mean = data_cell_[x][y].sum / data_cell_[x][y].point_num;
            data_cell_[x][y].x_sum += px;
            data_cell_[x][y].x = data_cell_[x][y].x_sum / data_cell_[x][y].point_num;
            data_cell_[x][y].y_sum += py;
            data_cell_[x][y].y = data_cell_[x][y].y_sum / data_cell_[x][y].point_num;
            // for (int i = 0; i < row; i++)
            // {
            //     for (int j = 0; j < col; j++)
            //     {
            //         if(data_cell_[i][j].vaild == 1 || data_cell_[i][j].z_mean < -3)
            //             continue;
            //         GetValid(i, j, 1, data_cell_);
            //     }
            // }
            // 只对十米以内的障碍物进行筛选,十米以上的障碍物不关注高度,都做为障碍物,十米以内必须要大于0.3
            // this->DistValid(x, y, data_cell_);
            return 0;
        }
        // 用于没有接收到点云时输出结果,需要将上一帧的数据 这个函数暂时没有用
        int LidarCellsGrid::PointsCa(int i, int j, std::vector<std::vector<data_cell>> &data_cell_, std::vector<std::vector<data_cell>> &data_cell_tmp)
        {
            if (data_cell_tmp[i][j].point_num > data_cell_[i][j].point_num)
            {
                data_cell_[i][j].point_num = data_cell_tmp[i][j].point_num;
            }
            else
            {
                data_cell_tmp[i][j].point_num = data_cell_[i][j].point_num;
            }
            return 0;
        }
        // 根据距离和障碍物高度判断栅格的有效性
        int LidarCellsGrid::DistValid(int x, int y, std::vector<std::vector<data_cell>> &data_cell_)
        {
            float x_ = float(x) * cell_config_.cell_size_x + cell_config_.xmin;
            if (x_ < cell_config_.dist10)
            {
                if (data_cell_[x][y].z_intercept > cell_config_.dist10_th)
                {
                    data_cell_[x][y].vaild = 1;
                    auto time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
                    data_cell_[x][y].time = time.count();
                }
            }
            else if (x_ >= cell_config_.dist10 && x_ < cell_config_.dist20)
            {
                if (data_cell_[x][y].z_intercept > cell_config_.dist20_th)
                {
                    data_cell_[x][y].vaild = 1;
                    auto time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
                    data_cell_[x][y].time = time.count();
                }
            }
            else if (x_ >= cell_config_.dist20)
            {
                if (data_cell_[x][y].z_intercept > cell_config_.dist30_th)
                {
                    data_cell_[x][y].vaild = 1;
                    auto time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
                    data_cell_[x][y].time = time.count();
                }
            }
            return 0;
        }
        int LidarCellsGrid::GetConfidence(int x, int y, std::vector<std::vector<data_cell>> &data_cell_, std::vector<std::vector<data_cell>> &data_cell_tmp)
        {
            // 无效的cell进行减小置信度
            if (!data_cell_[x][y].vaild)
            {
                data_cell_[x][y].confidence -= cell_config_.confidence_;
                if (data_cell_[x][y].confidence < 0)
                    data_cell_[x][y].confidence = 0;
                return 0;
            }
            // 有效的cell继续增加置信度,根据距离得到的置信度比例
            float confidence_ = data_cell_[x][y].point_num * cell_config_.confidence;
            data_cell_[x][y].confidence = confidence_ + data_cell_tmp[x][y].confidence;
            if (data_cell_[x][y].confidence > 30)
                data_cell_[x][y].confidence = 30;
            return 0;
        }
        // 根据密度判断有效性
        int LidarCellsGrid::GetDensity(int x, int y, std::vector<std::vector<data_cell>> &data_cell_)
        {
            float must_points_num = 1 - cell_config_.density * x;
            if (data_cell_[x][y].point_num < must_points_num)
                data_cell_[x][y].vaild = 0;
            return 0;
        }
        // 根据高度差判断有效性
        int LidarCellsGrid::GetIntercept(int x, int y, std::vector<std::vector<data_cell>> &data_cell_)
        {
            float x_ = std::fabs(x * cell_config_.cell_size_x + cell_config_.xmin);
            float must_intercept = 1 - cell_config_.intercept * x_;
            if (data_cell_[x][y].z_intercept < must_intercept)
                data_cell_[x][y].vaild = 0;
            return 0;
        }
        int LidarCellsGrid::GetValid(int x, int y, int search_scope, std::vector<std::vector<data_cell>> &data_in)
        {
            if (data_in[x][y].vaild)
                return 0;
            if (data_in[x][y].z_mean > cell_config_.obj_height)
            {
                data_in[x][y].vaild = 1;
                data_in[x][y].confidence = 1;
            }
            // for (int i = x - search_scope; i <= x + search_scope; i++)
            // {
            //     for (int j = y - search_scope; j <= y + search_scope; j++)
            //     {
            //         if (i < 0 || i >= row || j < 0 || j >= col || (i == x && j == y) || data_in[i][j].vaild == 0)
            //             continue;
            //         if (data_in[x][y].z_min > data_in[i][j].z_min || std::fabs(data_in[x][y].z_min - data_in[i][j].z_min) < 0.2)
            //         {
            //             data_in[x][y].vaild = 1;
            //             data_in[x][y].confidence = 1;
            //             return 0;
            //         }
            //     }
            // }
            return 1;
        }

    }
}